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ABSTRACT 


The work reported here demonstrates how to automatically compute the position 
and attitude of a TRAC camera relative to the robot end effector. In the robotics 
literature this is known as the sensor registration problem. The registration problem is 
important to solve if TRAC images need to be related to robot position. Previously, 
when TRAC operated on the end of a robot arm, the camera had to be precisely 
located at the correct orientation and position. If this location is in error, then the 
robot may not be able to grapple an object even though the TRAC sensor indicates it 
should. In addition, if the camera is significantly far from the alignment it is expected 
to be at, TRAC may give incorrect feedback for the control of the robot. A simple 
example is if the robot operator thinks the camera is right side up but the camera 
is actually upside down, the camera feedback will tell the operator to move in an 
incorrect direction. 

The automatic calibration algorithm requires the operator to translate and rotate 
the robot arbitrary amounts along (about) two coordinate directions. After the mo- 
tion, the algorithm determines the transformation matrix from the robot end effector 
to the camera image plane. 

This report discusses the TRAC sensor registration problem. Different aspects of 
this work has been published in [1, 2, 3] and submitted for publication in [4]. 

INTRODUCTION 

When you mount a sensor on a robot it becomes necessary to find the pose (orien- 
tation and position) of the sensor relative to the robot. This is the sensor registration 
problem. 

Many researchers have provided closed-form solutions to the sensor registration 
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problem; however the published solutions apply only to: (1) sensors that can measure 
a complete pose (three positions and three orientations) and (2) robots that can move 
precise distances. Neither of these requirements exists for our application. 

Unless the TRAC is aligned with a mirror, it can provide only position information. 
If you assume the registration problem has not yet been solved, then it may not be 
possible to move the robot to align to a mirror. 

If the robot is actually a velocity commanded telemanipulator (as in the SRMS), 
it is possible to enter a joystick command to move along an axis, but the precise 
distance moved may be awkward to determine. As a result it is necessary to solve 
the registration without precise knowledge of the robot’s motion. 

This report provides a closed-form solution to the sensor registration problem 
applicable when: (1) the sensor can provide at least position information, and (2) the 
robot can move on and rotate about straight lines. 

FORMULATING THE PROBLEM 

This report assumes U T V is a 4 by 4 homogeneous transformation matrix describing 
the coordinate transformation from frame u to frame v, this is equivalent to the pose 
of v relative to the pose of u. Also, this report assumes sensors, such as a range 
sensor, a camera, or a fixture, are fixed relative to tool, t , while in use but may be 
removable. Thus, the frame of the sensor relative to the tool (represented as l T a 
which is what we desire to determine) is constant while in use. 

We will discuss the pose of various objects at several manipulator positions. To 
designate what robot position we axe referring to, we will use a subscript on the 
transformation matrix. Some matrices are constant regardless of the robot position. 
Constant transformations will have no subscripts. For example, to designate a trans- 
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formation (“T") when the robot is at location i, we will use U T?. The transform when 
the robot is at location j is U TJ . The relative change in the transform relative to u 
is: 



In an application, suppose a robot has a feedback sensor mounted on its tool. 
When the robot is positioned at location t, the pose relative to the sensor frame of an 
object fixed in g is represented with the matrix S T°. Furthermore, we presume that 
the tool relative to the sensor frame, ( 8 T l ), is unknown but desired. The problem is 
formulated as follows: 


grpo grjittrj-\8Srpo 


( 1 ) 


Now, if the manipulator is positioned at j , we have the following: 


grjpo grj^ttrp88rj^O 


As shown in the transform graph of Figure 1, we can formulate the following: 


t'J^ggrpHrjps trpssrpoorpa 


(2) 


Equation 2 is of the form AX = XB which shows it to be the sensor registration 
problem [6] and [7, 8] also [9, 10]. The B transform is typically measured, while 
the A transform is computed from joint angles. The objective is to solve for X 
given A and B. As shown in Figure 1, the transformations A and B describe the 
relative displacements of an arbitrary movement (from i to j) of the tool frame ( 9 T‘) 
and sensor frame ( 3 T ° ), respectively. The constant matrix X in Figure 1, is the 
relative transformation between the tool frame and the sensor frame. The published 
solutions for X, including the analytical ones, are valid only if: (1) the sensor provides 
complete pose 1 data and the robot provides complete tool pose accurately. This full 
1 Complete pose measurement requires three position and three orientation components. 


3 



X 


9 



tj X 



Figure 1: An Arbitrary Movement of the Tool of a Robot and Formulating a Homo- 
geneous Transform Equation of the Form AX = XB. 


pose requirement means that matrices A and B are completely known and invertible. 
Thus, in situations, where either A or B is not fully known, the registration problem 
is more complex, and the published solutions cannot be applied. 


THE GENERAL SOLUTION 


To find a solution to the registration problem, we need to manipulate the parts 
of the A, B and X matrices given in equation 2. The parts of the A matrix will be 
referenced as follows: 


R a 

Pa' 


’ ifti tpt 

9i-+j 

000 

1 


°00 1 


( 3 ) 


where *P^ is the 3 by 1, translation vector and ‘R* represents a 3 by 3 rotation 
part of A. The rotation and translation components of B will be represented as 
and ; the parts of U T” are u R” and U P?. The parts of X will be expressed as: 


X = 


R x P t 
000 1 


4 


T 


We will need parts of U R V so we will write : 2 


U 



« o v 



■ ujyt> T 

u O vT 

u G vT 


(4) 


We can express the homogeneous transformation equation = XB in terms of 
rotation and position transformation matrices as follows: 


' R a 

Pa' 

R x P x 


' R x P x ' 

' Rb 

P b ' 

0 

1 

0 1 


0 1 

0 

1 


Expanding equation 5, we have, 

R a R x = R x Rb (6) 


and, 


(R a — It) P x = R x Pb — P a 


where J 3 is a 3 by 3 identity matrix. 


(7) 


Solving for the Rotation Part of X, R x 


Lemma 1 If the robot translates the tool frame t, from location i, an amount (x, y, z) 
relative to frame t, then: 



2 It is common practice to express the rotational components as N (normal), O (orthogonal), and 
A (approach) directions. Using A for the third direction however, might cause confusion with the 
A matrix, the common notation used in the sensor registration problem. As a result, we must 
adopt a different notation for one or the other. We chose to call the third rotational vector, the 
Grip direction G. 
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First, suppose the robot is located at location * given by: 9 Tf. Next allow the robot 
to move to location j by translating (x,y,z); therefore, we can write: 


s r p t . = sj't 


This can be written as: 


t'j’pgj't — 


h 

000 

h 

000 


x 

y 

z 

1 


X 

y 

z 

1 


By the definition of A, we have: 


A- 



’ x ' 


h 

y 



z _ 


000 

1 



Q.E.D. 


Lemma 2 If the robot translates the tool frame t, from location i, an amount (x, y, z) 
relative to frame t, then: 


B = 


I z S P- - *P° 
000 1 


Let the tool be located at i and the object’s location relative to the sensor be: 

*T° = 


S R? 6 P9 


000 1 

Suppose the tool frame is translated until it reaches location j and the object relative 
to the sensor is given by: 


SrpO 

1 3 


8 R°‘ 8 P? 


000 1 

By definition we have B = a T°°Tf, from lemma 1 we know = h, hence from 

equation 6 we can write: 


D ns r>8 

rtx 


°\-*j 
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Figure 2: The Tool Translates Along Its x-axis; to compute the Unit Vector Nj of 
a TK 


Premultiplying by the inverse of R x , we find I 3 = *R% i . From the definition of B, 
we can expand the multiplication to express the translational part of B as *P* = 

a Ri°Pj 4- a P°. Now from the definition of the inverse of a transformation matrix we 


can write: 


srpo _ 


-1 


a R° 

000 



(8) 


Using the last two expressions together, we have 'P£ = — a Pj + a P°. Q.E.D. 

Now we will use these lemmas to solve for the rotational parts of X. As shown 
in Figure 2 equation 2, and lemma 1, we observe that if we translate an unknown 
distance A* along the x-axis of t, we can write equation 7 as: 


0 = R a P a 





If we express R x with its component parts, we have: 


1 

<1 



0 

— 

0- -’PL., 

0 J 


o. ■ -n,, J 


(9) 


where *P* = S P° — a P° is the relative position vector of the sensor frame from 

position i to position j and is measurable. Equation 9 says that a P ta ._^. is perpendicular 
to O x and G x . Therefore, s Po i ^ j (which is known from measurements) must be in the 
direction of N x . This implies N x equals a unit vector along s Po i _ tj - 
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Similarly, we can compute O x by translating the robot tool frame along the y axis 
of the tool an arbitrary unknown amount, A y . The third orientation quantity, G x , 
can be computed easily by translating the tool frame along the z-axis, or by using 
the orthogonal property of R x , that is, G x — N x x O t . 


Solving for the Translation Part of X 

Lemma 3 If the robot rotates the tool frame t, from location i, an amount given by 
R, about an axis oft, then: 

0 1 
0 

0 J 

000 1 



First, suppose the robot is located at location i given by: 9 Tf. Next allow the robot 
to move to location j by rotating about an axis through the origin of t. We can write: 


This can be written as: 


g T* = 9 T ■ 



' 0 ' 


R 

0 



. 0 . 


000 

1 



t'J'99'J't — 



' 0 ' 


R 

0 



. 0 . 


000 

1 



= A 


Q.E.D. 


Lemma 4 If the robot rotates the tool frame t, from location i, an amount given by 
R, about an axis oft, then: 


B = 


{(RJ-'RR"} {‘Pf-iRj-'RR/P?} 
000 1 
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Figure 3: The Tool Rotates About Its x Axis; to Compute the y and 2 Components 
of P x . 

From equation 6, we have a R a 0i ^ i = (-ft*) -1 . il*. Using lemma 3 we find a R a 0i _+ i = 

(R x )~ l RR X . From the definition of B, we have: 


9 p 8 — 3 p?°p? _j- 3 p? — _|. 8 p? 


Using equation 8 we can write: 


a pa 


= + a P? 


Q.B.D. 

Now we will determine P x . When t R t gi _ t . is not equal to I 3 , it has one and only 
one eigenvalue equal to 1; therefore, (^R t g .^ j — I 3 ) in equation 7 has a rank equal to 
two [6], Therefore, P x in equation 7 has one degree of freedom, and we can use it to 
solve for two components of P x . 

For example, if we rotate the tool frame with an amount 0 tx from location i about 
the x-axis of t, then t P g . ■ = 0 and equation 7 appears as follows (by lemma 3): 


' 0 

0 

0 



0 

cos (9 tx ) — 1 

-sm(9 tx ) 

[ p- ] = R -’ p L, 

(10) 

. 0 

sin(0 tx ) 

cos (9 tx ) - 1 _ 



Figure 3 shows the transformation graph described by equation 10. Equation 10 has 
no information for finding the x component of P x . However, we can resolve the y 
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and z components by equating the last two non-zero rows of equation 10. Note that 
provided the angle 9 tx is known, everything in equation 10 except P x is known. For 
example, R x has been determined already, and comes from lemmas 3 and 4. 

Equations 7 and 10 show that ( R a — I 3 ) becomes zero if the magnitude of rotation 
about any axis is zero or a multiple of 27T. Therefore, this condition should be avoided 
when solving the translation part of *T*. 

Similarly, you can compute the x component of P x by rotating the tool frame 
about either the y or z axis of frame t, then using the two non-zero rows of the 
equation resulting from 7. 

Solving for the Translation When Rotation Magnitude is Unknown 

During the investigation, we discovered another, more general, method for finding 
the translational components when the robot rotations are unknown. The transla- 
tional components can be determined after performing a minimum of three rotations 
about the tool frame. 

The left of Figure 4 shows that a rotation about the tool frame changes only the 
orientations of the tool and sensor frames, and not the object frames. This is due to 
the fact that the object is fixed. The right half of Figure 4 shows the effect of the 
same rotation relative to the tool frame. The position of the object relative to the 
sensor in both figures are the same. For example, let 9 Tq represent the position of 
the tool frame (relative to the ground) before any rotations are performed, and 9 T- is 
after rotation i. Now suppose the robot is moved so it rotates an angle 6 about the 
n axis of the tool (neither 9 nor n need to be known). We will represent the motion 
with the transform R# tn . After application of the motion, the object’s origin can be 
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°o.0i 


Object 

\ 




G 


(a) During rotation about t, only 
the sensor frame and t frames 
change. 



Figure 4: The Effect of a Rotation About the Tool Frame. 


computed as: 

t DO D t DO 

•T i — -*1-0, n ■*() 

which demonstrates l P° to be a point on a sphere centered at the origin of the tool. If 
four points on this sphere can be measured (implying a minimum of three rotations) , 
the sphere’s center (the origin of the tool) can be found. 

Since the sensor measures the position of the object relative to the sensor frame, 
measuring the object before and after the tool rotations enables the tool center relative 
to the sensor frame *P t to be computed. Once the center is found, l P s is computed 
as: 

t P a = -*R a4 P‘ (11) 

Computing the center of a sphere given four points is a simple and well defined 
problem. Singularities in the problem can be avoided if all four points are distinct 
and do not lie on a single plane. The method we used to compute the sphere center 
is performed using the following steps. 
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1. Number the points 1, 2, 3, and 4. A point is a 1 by 4 matrix, the first 3 elements 
are the x, y, z coordinates, the fourth element is 1. Let P, represent point i. 


2. Pair the points into three groups such as (1,2), (2,3), (3,4). 

3. Compute midpoints (m,) of each point group: 

P 1 + P 2 
mi = 2 

4. Compute unit vectors (v,) between the points in each pair: 

P2-P1 

Vl ~ \P2~Pl\ 

5. Compute the equation of planes (C,) normal to u,- containing m,: 

Ci = (vi,0)- (0,0,0 , vi -mi) 

6. Solve the linear equations for the sphere center (x,y,z): 

0 
0 
0 
0 

ALGORITHM 



This section summarizes the discussion in the previous sections and puts the 
material into algorithmic steps. 


To Compute Rotation 

To compute the rotation component of matrix a T *, translate the tool frame along 
two different axes, one at a time. 

1. Record the position of the object with respect to the sensor at position 1, a P°. 
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2. Translate the tool in the positive x-axis to position 2. Record the new position 
of the object with respect to the sensor, ‘Pg. 

3. Translate the tool in the positive y-axis to position 3 and record the position, 

8 JDO 

r 3- 


4. Compute the unit vectors of the rotation matrix of the sensor-to-tool frame, 
a R t , using: 


s N * = 


and 


3 DO 3 DO 

tLZ—Ll 

\ a p f - a p 2 °\ 

8 no 8 IDO 

3 Qt 2 £j 3 

~ i 'Pi - •p 3 ”i 


*G' = ’N‘ x 'O' 


( 12 ) 

(13) 

(14) 


5. Compute the rotation matrix of the tool-to-sensor frame: 1 R“ = ( s /?‘) 


To Compute Translation - Method 1 

To compute the translation part of a T t , rotate the tool frame about any two of its 
axes, one at a time. 

1. Compute the global to tool frame at position 4, (this is done using a forward 
kinematic model and joint location measurements) 9 T\, and record 3 P%. 

2. Rotate the tool about its x-axis to position 5. Compute 9 T^ and record a Pg. 

3. Compute the relative rotation of the tool frame from position 4 to 5, # = 

rotation part of 9 Tl. 

4. Compute the relative rotation of the sensor frame, a R a OA _ ¥i = R x ~ u Rg 4 ^ & R x . 
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5. Compute: 


(15) 


a pa 

^ 04-+5 


= -K^ 9p f + ap Z 


6. Compute the y and z components of P x by using equation 10. 

7. Rotate the tool frame about its y-axis to location 6 and repeat steps 2 through 5. 
Use a variant of equation 10 to compute the x component of P x . 

To Compute Translation - Method 2 

1. Record 34 P° at location 4. 

2. Rotate about the x-axis of the tool to location 5 and record the objects’ position, 

s$ pO 

3. Rotate about the y-axis of the tool to location 6 and record the object’s position, 

s« pO 

4. Rotate about the z-axis of the tool to location 7 and record the object’s position, 

57 pO 

5. Compute 4 S 6 P* at the sphere center of the above 4 points. 

6. Compute t P a = -'IP’P 1 . 


EXAMPLE 


We used a software simulation to test the algorithm proposed in this report. We 
imagine a position sensor on the last link of a robot manipulator for feedback control 
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application. The real position and orientation of the sensor frame relative to the tool 
frame (matrix X) is chosen as follows: 

0.925 -0.163 0.342 20 

t a 0.319 0.823 -0.470 -12 

~ -0.205 0.544 0.814 10 

0 0 0 1 

The solution for X requires movements of the tool frame 9 T t \ translations and 
rotations. In practice, the tool pose 9 T * and the object’s position relative to the sensor 
( S P° in this example) are determined from the information provided by the robot’s 
controller and the sensor. In simulation, however, 9 T * and a P° are computed by using 
equations: 9 Tj = 9 Tt t T*._>. and a Pf = sp°, where is an assumed 

movement (either translation, rotation or both) of the tool frame from position i to 
position j, and 9 P° is an assumed position relative to the global frame g of a fixed 
object o. 

In the following simulation, the 9 T° was chosen as: 

' 1 0 0 20 

grpO _ 0 1 0 -15 

0 0 1 10 

0 0 0 1 

The initial robot position ( 9 T{) was chosen as: 

'001 2 
t= 10 0-5 
1 0 10 3 

0 0 0 1 

Method 2 - When Robot Rotations are Unknown 

The required positions of the tool frame are chosen as: (1) Given above, (2) 
Translate 10 units along the x-axis from position 1, (3) Translate 5 units along the 
y-axis from position 2, (4) Equal to point 1, (5) Rotate 30 degrees about the x-axis 
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from position 4, and (6) Rotate 20 degrees about the y-axis from position 5, (7) 
Rotate 40 degrees about the z-axis from position 6. 


These positions axe: 

0 0 1 2 

9T t = 10 0-5 

1 0 10 3 

0 0 0 1 

‘ 0 0 1 2 ‘ 

9T t = 10 0 5 

2 0 10 3 

0 0 0 1 

'0012' 

9T t = 10 0 5 

3 0 10 8 

0 0 0 1 

'001 2 
9T t = 10 0-5 

4 0 10 3 

0 0 0 1 

' 0 0.5 0.866 2. 

, 1. 0 0 -5. 

5 0 0.866 -0.5 3. 

0 0 0 1. 

' -0.296 0.5 0.814 2. 

9T t 0.940 0 0.342 -5. 

6 0.171 0.866 -0.470 3. 

0 0 0 1. 

' 0.0945 0.573 0.814 2. 

g t 0.720 -0.604 0.342 -5. 

7 “ 0.688 0.553 -0.470 3. 

0 0 0 1. 

The sensor output corresponding to each of these positions are: 

*P? = [ -23.3 24.9 -12.7 1 ] 
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a P 2 ° = [ -32.6 26.5 -16.1 1 ] 

*P° = [ -34.2 22.4 -13.7 1 ] 

a P£ = [ -23.3 24.9 -12.7 1 ] 

a P£ = [ -19.6 28.3 -21.3 1 ] 

a P£ = [ -22.0 26.6 -25.9 1 ] 

a P° = [ -8.44 28.8 -23.9 1 ] 


Computing the Unit Vectors of R x 


Using equations 12, 13 and 14, a N *, a O *, and a G * are found using the translational 
components of positions 1,2, and 3 as: 

\T 


•N* = [ 0.925 -0.163 0.342 0 ] J 
s O t = [ 0.319 0.823 -0.470 0 \ 


and 


a G t = [ -0.205 0.544 -0.814 0 ] T 
Using the orthonormal properties of R x (eg. t R a = a R tT ) we obtain: 


R x = 


0.925 -0.163 0.342 

0.319 0.823 -0.470 

-0.205 0.544 0.814 


(16) 


The translational components of points 4 through 7 lie on a sphere. The center 
of the sphere is computed to be: 

a P t = [ -12.6 7.70 -20.62 1. ] 

Using equations 16 and 11, we determine *P a as: 

t P a = = [ 20. -12. 10. -1. ] 
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Method 1 - When Robot Rotations Are Known 


In the following simulation, the 9 T° and initial positions 9 T[ are the same as 
before. The position 9 T\ (starting point for the rotations) was chosen to be equal to 
9 T{ given above. 

Translating the robot tool along the x-axis and y-axis, one at a time (same amounts 
as before), the respective positions, a P.£ a P$, are recorded as follows: 


*P? = [ -23.3 24.9 -12.7 1 ] T 
S P2 = [ —32.6 26.5 -16.1 1 ] T 


and 

^=[-24.9 20.8 -10.3 1 ] T 


At an initial position 4, record 9 T\ and *P 4 °. Rotate the tool about its x-axis a 
non- trivial amount and record 9 Tl and 3 P§. Rotate the tool from position 5 about 
its y-axis to position 6, and record fl Tg and a Pg . The resulting input values are as 


follows: 


9 Tl = 


0 0 12 
10 0-5 
0 10 3 

0 0 0 1 



0 0.5 0.866 2 

10 0-5 

0 0.866 -0.5 3 

0 0 0 1 



-0.296 0.5 0.814 2 

0.940 0 0.342 -5 

0.171 0.866 -0.470 3 

0 0 0 1 


*Pf = [ —24.3 24.4 -24.9 1 ] T 
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and 


II 

a? 

’ -22.1 

22.5 

-31.8 1 

T 

ii 

-20.5 

19.6 

-35.8 1 

T 


Using equations 12, 13 and 14, *N t , a O\ and *G t are found as: 
a N* = [ 0.925 -0.163 0.342 0 ] T 
a O* = [ 0.319 0.823 -0.470 0 f 

and 

a G l = [ -0.205 0.544 -0.814 0 ] T 

Using the orthonormal properties of R x (eg. t R a = ( a R*) T ) we obtain: 

' 0.925 -0.163 0.342 ' 

R x = 0.319 0.823 -0.470 

. -0.205 0.544 0.814 

Compute the relative rotation matrices of the tool for the two rotations: that is 
the rotation part of l T% 9 Tl and the rotation part of t Tj? 9 T£. They are found as: 

' 1 0 0. ' 

4 R 5 = 0 0.866 -0.5 

. 0 0.5 0.866 . 


' 0.940 0 0.342 ' 

5 R 6 = 0 10 

. -0.342 0 0.940 . 

By using equation 6 (eg. a R a 0i _+ j = R x u Rg._^.R x ), the relative rotations of the 
sensor are computed as: 

' 0.981 -0.191 -0.0392 ' 

= 0.151 0.870 -0.470 

L 0 124 0.455 0.882 
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and 


a 




0.946 0.176 0.272 ' 

-0.145 0.980 -0.132 
-0.290 0.0857 0.953 


Using equations 15, compute 4 P* 4 _ +8 and $ P£ t _ ¥e , and their values are 


a 


pa 

04-H 


' 0.422 ' 
-6.78 
-4.38 


( 17 ) 


and 


a pa 


' 3.57 ‘ 
-4.41 
. -5.30 . 


(18) 


Finally, compute the components of P x by using equations 10, 17 and 18. We find 


P x = (20, —12, 10) T . 


CONCLUSIONS 

When a TRAC sensor is mounted on a robot for feedback control applications, 
the sensor measures objects relative to the sensor frame. Since the ultimate objective 
is to know the tool frame with respect to the object, it is necessary to compute the 
sensor frame relative to the tool. Determining the tool-to-sensor frame is a form of the 
sensor registration problem confronted by researchers using tool mounted feedback 
sensors. The published solutions to the sensor registration problem are applicable 
only when the sensor is able to measure a complete pose and the robot tool pose is 
fully known. However, unless the TRAC is nominally normal to the mirror target, 
it can only measure position. Additionally, in the case of velocity driven robots (like 
the SRMS), there may be no convenient access to precise tool pose information. 

This report provides a closed-form solution for solving the sensor registration, 
l T s , when a position only sensor is used. The solution requires movements of the tool 
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frame a T t \ translations and rotations. 


The method has been demonstrated in simulation. Initial experiments have not 
been successful due to measurement errors. The work used a TRAC target at a 30 
foot range. Initial simulation studies indicate the sensitivity will improve significantly 
with a shorter range. Future work will investigate the sensitivity of the method and 
results will be forwarded to the sponsor. 
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